#include <iostream>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>  // 将ROS下的sensor_msgs/Image消息类型转化为cv::Mat数据类型
#include <sensor_msgs/image_encodings.h> // ROS下对图像进行处理
#include <image_transport/image_transport.h> // 用来发布和订阅图像信息

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio.hpp>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_node");  // ros初始化，定义节点名为camera_node
    ros::NodeHandle nh("~");                      // 定义ros句柄
    image_transport::ImageTransport it(nh);  //  类似ROS句柄
    image_transport::Publisher image_pub = it.advertise("cameraImage", 1);   // 发布话题名/cameraImage
    int img_width,img_height,mode;

    ros::Rate loop_rate(200);   // 设置刷新频率，Hz

    cv::Mat imageRaw;  // 原始图像保存
    cv::VideoCapture capture(0);   // 创建摄像头捕获，并打开摄像头0(一般是0,2....)

    if(capture.isOpened() == 0)      // 如果摄像头没有打开
    {
        std::cout << "Read camera failed!" << std::endl;
        return -1;
    }
    nh.param("img_width",img_width,640);
    nh.param("img_height",img_height,320);
    nh.param("img_mode",mode,0);
    ROS_INFO("\033[36m \nimg_width: %d \nimg_height: %d\nmode: %d\033[0m",img_width,img_height,mode);
    while(nh.ok())
    {

        capture.read(imageRaw);          // 读取当前图像到imageRaw
        cv::Mat img_0,img_1,img_2;
        cv::resize(imageRaw,img_1,cv::Size (img_width,img_height));

        if(mode == 1){
            cv::cvtColor(img_1,img_2,cv::COLOR_BGR2GRAY);
        }
        else if (mode == 0){
            img_2 = img_1.clone();
        }
        cv::imshow("image_show", img_2);  // 将图像输出到窗口
        sensor_msgs::ImagePtr  msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imageRaw).toImageMsg();  // 图像格式转换
        image_pub.publish(msg);         // 发布图像信息
        ros::spinOnce();                // 没什么卵用，格式像样
        loop_rate.sleep();              // 照应上面设置的频率
        cv::waitKey(33);
    }
}
